#pragma once
#include <stdio.h>
#include <stack>
#include <concurrent_queue.h>
#include <omp.h>

#define N_FOR_CRAWL 6

namespace PCS
{
	namespace Core
	{		
		/**Implements a concurrent (using OMP) RanSaC segmentation process*/
		class RansacManager
		{
		private:
			struct RanSacPlane{
				Point * surface_point_1;
				Point * surface_point_2;
				Point * surface_point_3;
				std::vector<Point *> * inliers;
			};

			SparseGrid * m_pGrid;

			std::vector<RanSacPlane*> * ransac_planes;
			
		public:			
			RanSacManager(void) {  }
			RanSacManager(SparseGrid * &p_pGrid) { m_pGrid = p_pGrid; }  

			void Search() {
				boost::timer timer;
				std::cout << "Searching for planes within " << m_pGrid->GetMap()->size() << " grid cells ..." << std::endl;
				int numberOfProcs = omp_get_num_procs();
				//numberOfProcs = 1;
				std::vector<Point*> * seeds = new std::vector<Point*>();
				std::map<std::string, std::vector<Point*>*> * map = m_pGrid->GetMap();
				int map_size = map->size();
				int step = (int) (map_size / (numberOfProcs + 0.5f));
				int crtCell = 0;

				Vector3 extent = m_pGrid->GetExtent();				
				Vector3 size = m_pGrid->GetSize();
				AxisAlignedBoundingBox * bounds = m_pGrid->GetBounds();
				int patch_id = 0;

				Concurrency::concurrent_queue<Point *> * same_patch_work_queue = new Concurrency::concurrent_queue<Point *>();
				Concurrency::concurrent_queue<std::pair<Point *, Point *>> * different_patch_work_queue = new Concurrency::concurrent_queue<std::pair<Point *, Point *>>();
				Concurrency::concurrent_queue<std::pair<int, int>> * neighbour_patches_queue = new Concurrency::concurrent_queue<std::pair<int, int>>();

				//Establish the seeds according to the number of processors
				#pragma region 
				boost::timer timer_0;
				std::cout << "Computing Seeds .. ";
				std::map<std::string, std::vector<Point*>*>::iterator cellIterator = map->begin();
				for (int i=0; i<numberOfProcs; i++) {
					for (int k=0; k<step; k++) cellIterator++;
					std::vector<Point*>::iterator pointIterator = cellIterator->second->begin();
					seeds->push_back(*pointIterator);
				}
				std::cout << boost::str(boost::format("done in %d seconds") % timer_0.elapsed()) << std::endl;
				#pragma endregion				

				//Construct a map (size of sgrid) storing for each cell a vector with references to it's surrounding cells
				#pragma region 
				boost::timer timer_1;
				std::cout << "Computing Adjacency Map .. ";
				std::map<std::string, std::vector<std::vector<Point*>*>*> cellNeighbours = std::map<std::string, std::vector<std::vector<Point*>*>*>();				
				std::string cellKey; int xIdxP, yIdxP, zIdxP;
				std::map<std::string, std::vector<Point*>*>::iterator gridIterator;
				for (std::map<std::string, std::vector<Point*>*>::iterator cellIterator = map->begin(); 
					 cellIterator != map->end(); ++cellIterator)
				{					
					//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
					std::vector<std::vector<Point*>*> * neighbourCells = new std::vector<std::vector<Point*>*>();					
					std::vector<std::string> splitVec;
					cellKey = (*cellIterator).first;
					boost::algorithm::split(splitVec, cellKey, boost::algorithm::is_any_of(":"), boost::algorithm::token_compress_on);					

					xIdxP = atoi(splitVec[0].c_str());
					yIdxP = atoi(splitVec[1].c_str());
					zIdxP = atoi(splitVec[2].c_str());

					for (int xIdx = xIdxP-1; xIdx < xIdxP+2; ++xIdx) {
						for (int yIdx = yIdxP-1; yIdx < yIdxP+2; ++yIdx) {
							for (int zIdx = zIdxP-1; zIdx < zIdxP+2; ++zIdx) {
								cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % yIdx % zIdx);												
								if ((gridIterator = map->find(cellKey)) != map->end())	{																		
									neighbourCells->push_back((*gridIterator).second);
								}	
							}
						}							
					}

					cellNeighbours[boost::str(boost::format("%d:%d:%d") % xIdxP % yIdxP % zIdxP)] = neighbourCells;
 				}
				std::cout << boost::str(boost::format("done in %d seconds") % timer_1.elapsed()) << std::endl;
				#pragma endregion				

				boost::timer timer_2;
				std::cout << "Computing Patches .. ";
				#pragma omp parallel for schedule(guided)		
				for (int i=0; i<numberOfProcs; i++) {
					std::cout << " [T" << omp_get_thread_num() << "]";
					Point * seed = seeds->at(i);
					int work_done = 0;

					#pragma omp critical(inc_patchid) 
					{
						patch_id = patch_id+1;
						seed->pcpId = patch_id;
					}

					//Calculate cell index in sparse grid
					int xIdxP = (int)floor((float)(((seed->position.X - (bounds->GetMinExtent().X))  * size.X) / extent.X));
					int yIdxP = (int)floor((float)(((seed->position.Y - (bounds->GetMinExtent().Y))  * size.Y) / extent.Y));
					int zIdxP = (int)floor((float)(((seed->position.Z - (bounds->GetMinExtent().Z))  * size.Z) / extent.Z));

					//Determine neighbours and start region growth - initialise work queue from seeds
					#pragma region
					std::vector<Point*> neighbours = std::vector<Point*>();
					m_pGrid->GetNeighbours(seed, *cellNeighbours[boost::str(boost::format("%d:%d:%d") % xIdxP % yIdxP % zIdxP)], neighbours, N_FOR_CRAWL);
					std::vector<Point*>::iterator start = neighbours.begin();
					std::vector<Point*>::iterator end;
					if (neighbours.size() > N_FOR_CRAWL) end = neighbours.begin()+N_FOR_CRAWL;
					else end = neighbours.end();
					for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
						if ((*pointIterator)->T == seed->T) {
							if ((*pointIterator)->pcpId == -1) {
								(*pointIterator)->pcpId = seed->pcpId;						
								same_patch_work_queue->push(*pointIterator);
							}
						} else {
							if ((*pointIterator)->pcpId == -1) {
								different_patch_work_queue->push(std::make_pair(seed, *pointIterator));
							}
						}
					}
					#pragma endregion

					//Work Queues initialised - all threads should now start processing them.
					std::pair<Point *, Point *> * p_seed = new std::pair<Point *, Point *>();
					//bool first_timer = true;
					while (!same_patch_work_queue->empty() || !different_patch_work_queue->empty()) {
						while (same_patch_work_queue->try_pop(seed)) {
							xIdxP = (int)floor((float)(((seed->position.X - (bounds->GetMinExtent().X))  * size.X) / extent.X));
							yIdxP = (int)floor((float)(((seed->position.Y - (bounds->GetMinExtent().Y))  * size.Y) / extent.Y));
							zIdxP = (int)floor((float)(((seed->position.Z - (bounds->GetMinExtent().Z))  * size.Z) / extent.Z));
							m_pGrid->GetNeighbours(seed, *cellNeighbours[boost::str(boost::format("%d:%d:%d") % xIdxP % yIdxP % zIdxP)], neighbours, N_FOR_CRAWL);					
							start = neighbours.begin();
							if (neighbours.size() > N_FOR_CRAWL) end = neighbours.begin()+N_FOR_CRAWL;
							else end = neighbours.end();
							for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
								if ((*pointIterator)->T == seed->T) {
									if ((*pointIterator)->pcpId == -1) {
										(*pointIterator)->pcpId = seed->pcpId;
										same_patch_work_queue->push(*pointIterator);
									} else { 
										if ((*pointIterator)->pcpId != seed->pcpId) 
											neighbour_patches_queue->push(std::make_pair(seed->pcpId, (*pointIterator)->pcpId)); 
									}
								} else {
										if ((*pointIterator)->pcpId == -1) {
											different_patch_work_queue->push(std::make_pair(seed, *pointIterator));
										}
								}
							}
							work_done++;
						}

						#pragma omp critical(inc_patchid)
						{
							if (different_patch_work_queue->try_pop(*p_seed)) {
								if (p_seed->second->pcpId == -1) {								 								
									patch_id = patch_id+1;
									p_seed->second->pcpId = patch_id;
								}
								same_patch_work_queue->push(p_seed->second);							
							}
						}

					}
					
					//std::cout << "Thread #" << omp_get_thread_num() << " done - processed " << work_done << " points" << std::endl;
				} //concurrent for loop end	
				std::cout << boost::str(boost::format(" done in %d seconds") % timer_1.elapsed()) << std::endl;
				//Now check whether there are touching surfaces to merge
				

				std::cout << boost::str(boost::format(" all done in %d seconds") % timer.elapsed()) << std::endl;
			} // Build end
			
			
		};		

	}
}